/*
 * step.c
 *
 *  Created on: 2015-03-12
 *      Author: root
 */

#include "step.h"
#define queuesize 100
#define mDel 57
#define EarlyDrop 250
char turnQueue[queuesize];
char locaQueue[queuesize];

int dbg=0;

int gQueueLength=0;

int _gStepRemain=0; //50 or 100
int _gStepCalibrate=1;
int _gCalibrateRemain=400;


int _gCurrentTurn=0;
int _gCurrentStep = 0;
int _gStepPos = 0;
int _gStepDir=0;

int _gSC=1;
int _gDrop=0;

int sortCount=0;

int offset=0;

int basePeriod=100;

int _gEndPos=0;

int startDelay=20;
SList *l;

//List *l;

//First  50
//230,175,148,133,120,110,102,95,86,82,78,73,69,65,62,59,55,52
int _gStepMap[] = {
		175,148,133,120,110,102,95,86,82,78,73,69,65,62,59,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,59,62,65,69,73,78,
		82,86,95,102,110,120,133,148,175,
		//Second 50
		175,148,133,120,110,102,95,86,82,78,73,69,65,62,59,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,59,62,65,69,73,78,
		82,86,95,102,110,120,133,148,175,
		//The 100
		175,148,133,120,110,102,95,86,82,78,73,69,65,62,59,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,
		59,62,65,69,73,78,82,86,95,102,110,120,133,148,175,
		//padding
		100,100,100,100,100
};
/*
int _gStepMap[] = {
		175,125,107,91,82,74,69,62,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,62,69,74,82,91,107,125,175,
		//Second 50
		175,125,107,91,82,74,69,62,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,62,69,74,82,91,107,125,175,
		//The 100
		170,148,133,120,110,102,95,86,82,78,73,69,65,62,59,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,
		59,62,65,69,73,78,82,86,95,102,110,120,133,148,175,
		//padding
		100,100,100,100,100
};
 *//*
int _gStepMap[] = {
		200,164,137,117,102,91,83,77,73,69,67,65,64,63,62,
		61,61,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,61,62,65,69,73,78,
		82,86,95,102,110,120,133,148,175,
		//Second 50
		200,164,137,117,102,91,83,77,73,69,67,65,64,63,62,
		61,61,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,61,62,65,69,73,78,
		82,86,95,102,110,120,133,148,175,
		//The 100
		200,164,137,117,102,91,83,77,73,69,67,65,64,63,62,
		61,61,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,mDel,
		mDel,mDel,mDel,mDel,
		61,62,65,69,73,78,82,86,95,102,110,120,133,148,175,
		//padding
		100,100,100,100,100
};*/

char msg[] = {"Step "};

//internal functions
void initStepTimer(void);
void initStepGPIO(void);
void smallStep(int);
void step(int dir);
void stepPos(int dir);

int popQueue(void);
int StepControl2(void);
int queueSize=0;
int firstRun=1;
void zeroStepper(int);
//end Internal Functions


//Externally Accessible Functions

void initStepper(void)
{
	//set up peripherals
	initStepGPIO();
	initStepTimer();

	//LL

	rtnLink = NULL;
	newLink = NULL;
	rtnLink=0;
	newLink=0;

	setup(&head, &tail);
	gDC=1;
	_gSC = 1;
	l = List_createStep();

}

void stepperHandler(void)
{
	if (_gStepCalibrate)
	{
		int temp=basePeriod*200;
		step(1);
		ROM_TimerLoadSet(TIMER2_BASE, TIMER_A, temp);//always multiply by base period
		if ((gStateChange[4]) && (gOutput[4]==0))
		{
			_gStepCalibrate=0;
			_gEndPos=1;
			_gStepPos=0;
			ROM_TimerIntClear(TIMER2_BASE, TIMER_TIMA_TIMEOUT);
			//print_ready();
		}
		ROM_TimerIntClear(TIMER2_BASE, TIMER_TIMA_TIMEOUT);
	}
	else {
		//int temp=StepControl();
		int temp=0;
		temp=StepControl2()*1;//Derate
		temp=temp*basePeriod + (sortCount*basePeriod/4);//sort count accommodates for added pieces, currently by adding a scalar delay
		ROM_TimerLoadSet(TIMER2_BASE, TIMER_A, temp);//always multiply by base period
		ROM_TimerIntClear(TIMER2_BASE, TIMER_TIMA_TIMEOUT);
	}
}
int StepControl2(void)	//Hungry Stepper Nom-Nom
{
	int delayResult=100;
	//static int turnType=0;	//left,right,180 --1,2,3
	if (startDelay)
	{
		delayResult=100;
		startDelay--;
	//	bt_UARTsend("D...\r\n");
	}
	else {
		if (_gStepRemain)
		{
			step(_gStepDir);
			stepPos(_gStepDir);
			offset=(_gCurrentTurn)*50;

			delayResult=_gStepMap[offset-_gStepRemain];
			_gStepRemain--;
		}
	}
	if ( (gStateChange[4]) && (gOutput[4]) ) { zeroStepper( _gCurrentTurn ); }

	if (_gStepRemain == EarlyDrop ) { _gDrop=1; }

	if ( (_gStepRemain==0) && (_gSC==0) )	//Finish turn condition
	{
		_gSC=1;
		_gStepDir=0;
		_gCurrentTurn=0;
		delayResult=2*mDel;	//load Minimum Delay delay
		startDelay=25;
	}
	return delayResult;
}

void stepLeft(void)
{
	_gStepRemain=50;
	_gStepDir=1;
}
void stepRight(void)
{
	_gStepRemain=50;
	_gStepDir=0;
}
void step180(void)
{
	_gStepRemain=100;
	_gStepDir=1;
}
void smallStep(int coil)
{
	//define pins here for future reassignment
#define E1 PA_7 	//PB_5
#define L1 PA_6		//PB_0
#define L2 PA_5		//PB_1
#define E2 PD_3		//PE_4
#define L3 PD_2		//PE_5
#define L4 PD_1		//PE_1
#define Low 0x00

	switch(coil)
	{
	case 1: {
		ROM_GPIOPinWrite(GPIO_PORTA_BASE, (E1 | L1 | L2), (E1 | L1));
		ROM_GPIOPinWrite(GPIO_PORTD_BASE, (E2 | L3 | L4), E2);
	} break;
	case 2: {
		ROM_GPIOPinWrite(GPIO_PORTA_BASE, (E1 | L1 | L2), E1);
		ROM_GPIOPinWrite(GPIO_PORTD_BASE, (E2 | L3 | L4), (E2 | L3));

	} break;
	case 3: {
		ROM_GPIOPinWrite(GPIO_PORTA_BASE, (E1 | L1 | L2), (E1 | L2));
		ROM_GPIOPinWrite(GPIO_PORTD_BASE, (E2 | L3 | L4), E2);
	} break;
	case 4: {
		ROM_GPIOPinWrite(GPIO_PORTA_BASE, (E1 | L1 | L2), E1);
		ROM_GPIOPinWrite(GPIO_PORTD_BASE, (E2 | L3 | L4), (E2 | L4));

	} break;
	default: bt_UARTsend("SmallStep Failed"); UARTsend("Init Step...Fail\r\n");
	}
}

void step(int dir)
{
	if (dir)
	{
		_gCurrentStep++;
		if (_gCurrentStep > 4) { _gCurrentStep=1; }
	}
	else
	{
		_gCurrentStep--;
		if (_gCurrentStep < 1) { _gCurrentStep=4; }
	}
	smallStep(_gCurrentStep);
}
void stepPos(int dir)
{
	if (dir)
	{
		_gStepPos++;
		if (_gStepPos >199) { _gStepPos=0; }
	}
	else
	{
		_gStepPos--;
		if (_gStepPos < 0) { _gStepPos=199; }
	}
}
//------------------------------------------------------------------------



int popQueue(void)
{
	int listValue=0;
	int CP=0;
	int temp2=0;
	int delta=0;
	int stepDelta=0;
	int result=0;
	int temp=0;
	if (dbg) { bt_UARTsend("\t\tSize: "); }
	int l_size = List_lengthStep(l);
	if (dbg) { bt_UARTsend(itoa(l_size,10)); }
	if (l_size)
	{
		if (dbg) { bt_UARTsend("\tDequeuing: "); }
		listValue=List_popStep(l,0);
		//dequeue(&head,&tail,&rtnLink);
		temp2=_gStepPos;
		if (dbg) { bt_UARTsend(itoa(temp,10)); }
		if (dbg) { bt_UARTsend("\r\n"); }

		temp2+=23;

		if ( temp2 > 199) { temp2-=200; }
		//Position decoder
		if (temp2 >= 0)		{	CP=1;	}	//Black
		if (temp2 > 49) 	{	CP=2;	}	//Aluminum
		if (temp2 > 99)		{	CP=3;	}	//White
		if (temp2 > 149)	{	CP=4;	}	//Steel


		if (temp2 > 200)	{	bt_UARTsend("SPD Error: _gStepPos over 199\r\n");	}
		dbg=0;
		if (dbg) { bt_UARTsend("\t_temp2: "); }
		if (dbg) { bt_UARTsend(itoa(temp2,10)); }
		if (dbg) { bt_UARTsend("\r\n"); }
		dbg=0;
		delta = listValue - CP;
		if ( delta < 0)			{	delta +=4;	}

		if (dbg) { bt_UARTsend("\tDelta: "); }
		if (dbg) { bt_UARTputc(48+delta);
		if (dbg) { bt_UARTsend("\r\n"); }
		}
		//stepDelta = (eTest.itemCode - 1)*50 - _gStepPos;

		if (delta == 1)	//90(+)
		{
			_gStepRemain=50;
			_gStepDir=1;
			_gCurrentTurn=1;
		}
		else if (delta == 2)	//180
		{
			_gStepRemain=100;
			_gStepDir=1;
			_gCurrentTurn=4;
		}
		else if (delta == 3)	//90(-)
		{
			_gStepRemain=50;
			_gStepDir=0;
			_gCurrentTurn=2;

		}
		else if (delta == 0)	//0
		{

			//_gSC=1;
			_gSC=1;
			startDelay=25;
			_gDrop=1;
			/*			_gStepRemain=1;
			_gStepDir=1;
			_gCurrentTurn=4;*/
		}
		result=1;
		if (dbg) { bt_UARTsend("\t_gSR: "); }
		if (dbg) { bt_UARTsend(itoa(_gStepRemain,10)); }
		if (dbg) { bt_UARTsend("\r\n"); }
	}
	else
	{
		result=0;
	}
	return result;
}
int pushQueue(int item)
{
	List_appendStep(l, item);
	int result=0;

	return result;
}

//------------------------------------------------------------------------

//Santos L Halper Functions
void initStepTimer(void)	//init timer
{
	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER2);
	ROM_TimerConfigure(TIMER2_BASE, TIMER_CFG_PERIODIC);	//set timer for periodic mode
	basePeriod = (ROM_SysCtlClockGet() / (1000000/basePeriod));		//for 100uS
	//basePeriod+=(basePeriod/10);							//BasePeriod Fiddle Factor
	ROM_TimerLoadSet(TIMER2_BASE, TIMER_A, basePeriod*100 - 1);


	//Timer interrupt setup
	ROM_TimerIntClear(TIMER2_BASE, TIMER_TIMA_TIMEOUT);	//clear the interrupt
	ROM_TimerIntEnable(TIMER2_BASE, TIMER_TIMA_TIMEOUT);	//Enable the timer interrupt
	ROM_TimerEnable(TIMER2_BASE, TIMER_A);				//Enable the timer
	ROM_IntEnable(INT_TIMER2A);							//enable the interrupt
	bt_UARTsend("Init Timer2");
	loading_bar(24);
	load_okfail(1);
	memset(turnQueue,0,queuesize);

}

void initStepGPIO(void)
{
	ROM_GPIOPinTypeGPIOOutput(GPIO_PORTD_BASE, (PD_1 | PD_2 | PD_3));//Enable GPIO
	ROM_GPIOPinTypeGPIOOutput(GPIO_PORTA_BASE, (PA_5 | PA_6 | PA_7));//Enable GPIO
}

void zeroStepper(int oneEighty)
{
	if ( oneEighty )	//180 step
	{
		gStateChange[4] = 0;
		//if (_gStepRemain > 30 && _gStepRemain < 70 ) { _gStepRemain=50; }
		_gStepPos=0;	//Reset position tracker to 0
	}
	else				//90 step
	{
		gStateChange[4] = 0;
		if (_gStepRemain > 24) { _gStepRemain=50; }
		if (_gStepRemain < 25) { _gStepRemain=0; }
		_gStepPos=0;	//Reset position tracker to 0
	}
}


void printQueueSize(void)
{
	int temp=0;
	bt_UARTsend("\t\t\tQueueSize: ");
	temp = size(&head,&tail);

	bt_UARTsend(itoa(temp,10));
	bt_UARTsend("\r\n");
}
void printllQueue(link **h, link **t)
{
	link *temp;
	//int numElements;
	temp = *h;
	bt_UARTsend("\t\t\tQueue: ");
	while (temp != NULL)
	{
		bt_UARTputc(48+temp->e.itemCode);
		bt_UARTputc(' ');
		temp = temp->next;
	}
	bt_UARTsend("\r\n");
}
